# File: task1_env0329.py (完整改进版)    F-16C实验
# 任务1：起飞阶段完成起飞后最快到达第一个航点
# 任务2：起飞阶段最快起飞
# 任务3：非配平状态下的增稳起飞控制

import numpy as np
import math
import time
import gym
from gym import spaces
from getDCSdata import get_dcs_data  # 从getDCSdata模块导入获取DCS数据的函数
from keyboard_control import *  # 从keyboard_control模块导入所有函数，用于控制飞机
import matplotlib.pyplot as plt  # 用于可视化
import logging
import csv
from FlightDataLogger import FlightDataLogger

logger = logging.getLogger(__name__)  # 获取日志记录器


def dms_to_decimal(degrees, minutes, seconds):  # 度分秒转换为十进制度数
    '''
    degrees, minutes, seconds  = 度  分   秒
    '''
    # 符号处理
    is_negative = degrees < 0
    degrees = abs(degrees)

    # 转换为十进制度数
    decimal_degrees = degrees + minutes / 60 + seconds / 3600

    # 返回符号
    if is_negative:
        decimal_degrees = -decimal_degrees

    return decimal_degrees


# 目标航点坐标以及到达目标航点的速度
TARGET_LONG = dms_to_decimal(42, 42, 55)  # 经度  修改为第一个航点的经纬度即可
TARGET_LAT = dms_to_decimal(42, 00, 24)  # 纬度
TARGET_ALT = 4500.00  # 高度
TARGET_VA = 200  # 速度   200 m/s == 720 km/h

# 初始位置（科布列季机场跑道）经纬度以及高度
# INIT_LONG = dms_to_decimal(41, 51, 46)
# INIT_LAT = dms_to_decimal(41, 55, 48)
# INIT_ALT_K = 500 # 初始地点在空中的高度
INIT_ALT = 18  # 需要进行更改  机场跑道高度
# 飞机在跑道上的真实航向
INIT_HEADING = 64  # 需要进行更改  初始航向

# 最小起飞速度
MIN_TAKEOFF_SPEED = 55

# CSV文件路径，用于记录训练日志
CSV_FILE_PATH = "results//training_log.csv"

# 配置日志
logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
    handlers=[
        logging.FileHandler("results//training.log"),
        logging.StreamHandler()
    ]
)


class TakeoffEnv(gym.Env):
    def __init__(self):
        # 动作空间：连续动作空间，包含6个维度，分别对应油门、俯仰和滚转控制
        self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(6,), dtype=np.float32)
        # 观察空间：包含飞机的各种状态信息，共XX个维度，包括速度、姿态、位置等   ----33个维度
        self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(33,), dtype=np.float32)

        self.last_distance = None  # 上一时刻到目标航点的距离
        self.last_altitude = None  # 上一时刻的高度

        self.steps = 0  # 当前步数
        self.current_episode = 0  # 当前回合数
        self.total_reward = 0  # 用于记录每个回合的总奖励
        self.is_on_ground = True  # 是否在地面上
        self.action = None
        self.last_pitch_action = 0.0
        self.last_pitch = 0.0
        self.last_bank = 0.0

        self.smoothed_action = [0] * 6  # 初始化平滑动作
        self.last_action = [0] * 6

        self.heading_error = 0

        # 初始化CSV文件，写入表头
        self._init_csv_file()

        # 将当前坐标与目标坐标之间绘制直线
        self.reference_path = None  # 存储参考路径的经纬度点 全局参考路径
        self.path_generated = False  # 标志是否已生成路径
        self.flight_trajectory = []  # 记录飞行轨迹

        # 新增：记录飞行数据
        self.flight_data = []  # 存储飞行数据
        self.initial_position = None  # 初始位置
        self.target_position = (TARGET_LONG, TARGET_LAT, TARGET_ALT)  # 目标位置

        # 新增：航线规划相关
        self.waypoints = []  # 存储航线中间点 (经度, 纬度, 高度)
        self.current_waypoint_idx = 1  # 当前目标航点索引  从第二个航点开始
        self.reference_path = None  # 存储参考路径的经纬度点
        self.path_generated = False  # 标志是否已生成路径
        self.i = 1

        self.last_ground_speed = None
        self.last_heading = None
        self.last_action_stored = None

        super().__init__()
        self.data_logger = FlightDataLogger()  # 初始化数据记录器

    def _init_csv_file(self):
        """初始化回合数与奖励的CSV文件，写入表头"""
        with open(CSV_FILE_PATH, mode='w', newline='') as file:
            writer = csv.writer(file)
            writer.writerow(["Episode", "Step", "Reward", "Total Reward", "ALT", "IAS", ])

    def _plot_trajectory(self):
        """绘制飞行轨迹与参考路径对比"""
        if not self.reference_path or not self.flight_trajectory:
            return

        ref_longs, ref_lats = zip(*self.reference_path)
        traj_longs, traj_lats, _ = zip(*self.flight_trajectory)  # 使用飞行轨迹的经纬度

        plt.figure(figsize=(10, 6))
        plt.plot(ref_longs, ref_lats, 'b-', label='Reference Path')  # 蓝线表示参考路径
        plt.plot(traj_longs, traj_lats, 'g--', label='Flight Trajectory')  # 绿线表示飞行轨迹
        plt.scatter([TARGET_LONG], [TARGET_LAT], color='red', label='Target')  # 红点表示目标点
        plt.xlabel('Longitude')
        plt.ylabel('Latitude')
        plt.title('Flight Trajectory vs Reference Path')
        plt.legend()
        plt.grid(True)
        plt.savefig(f'trajectory_episode_{self.current_episode}.png')
        plt.close()
        logger.info(f"Flight trajectory saved as 'trajectory_episode_{self.current_episode}.png'")

    def reset(self):
        print("---------------------------调用reset重置环境---------------------------")
        self.current_episode += 1
        logger.info(f"Resetting environment (Episode {self.current_episode})")

        # 激活DCS窗口  重置环境不需要激活窗口
        activate_dcs_window()
        data = get_dcs_data()

        if data is None:
            logger.warning("Using fallback data after UDP timeout")
            data = self._get_fallback_data()  # 如果获取数据失败，使用模拟数据

        # 模拟非配平状态：随机初始角速度扰动
        data["Omega"] = [
            np.random.uniform(-0.0, 0.0),  # 滚转角速度 (rad/s)
            np.random.uniform(-0.0, 0.0),  # 俯仰角速度 (rad/s)
            np.random.uniform(-0.0, 0.0)  # 偏航角速度 (rad/s)
        ]
        logger.info(f"Non-trimmed state applied: Omega = {data['Omega']}")

        self.state = self._extract_state(data)  # 提取当前状态
        # self.last_distance = self._calculate_distance(data)  # 计算到目标航点的距离
        self.last_altitude = data["LLA_balt"]  # 记录当前高度
        self.steps = 0
        self.total_reward = 0  # 重置总奖励

        # 初始化 last_ground_speed
        self.last_ground_speed = data["VV_0"]  # 使用DCS数据的初始地面速度

        self.last_heading = data["heading_deg"]  # 初始化航向
        self.last_action_stored = np.zeros(6, dtype=np.float32)  # 初始化动作

        # 检测飞机状态
        if data["LLA_balt"] - INIT_ALT < 122.0:  # 400 英尺 == 120米
            self.is_on_ground = True
            logger.info("Aircraft detected on ground")
        else:
            self.is_on_ground = False
            logger.info("Aircraft detected in air")

        self.initial_position = (data["LLA_long"], data["LLA_lat"], data["LLA_balt"])
        # self.initial_position[0], self.initial_position[1], self.initial_position[2]
        self._generate_waypoints(self.initial_position[0], self.initial_position[1],
                                 self.initial_position[2])  # # 生成中间检查点
        # self._generate_waypoints(data["LLA_long"], data["LLA_lat"], data["LLA_balt"])  # # 生成中间检查点
        self._write_waypoints()  # 写入中间检查点到 CSV

        # 修改初始航点索引为 1（第二个检查点）
        if len(self.waypoints) > 1:
            self.current_waypoint_idx = 1  # 从第二个检查点开始
        else:
            self.current_waypoint_idx = 0  # 如果只有一个检查点，从第一个开始

        self.flight_trajectory = []  # 重置飞行轨迹
        self.flight_data = []  # 重置飞行数据

        # 生成优化航线（任务 1 改进）

        # self.current_waypoint_idx = 0  # 从第一个航点开始

        # 写入初始数据到 CSV
        self._write_flight_data(data)

        self.action = []

        self.last_pitch_action = 0.0
        self.last_pitch = 0.0
        self.last_bank = 0.0
        self.smoothed_action = [0] * 6
        self.last_action = [0] * 6
        self.heading_error = 0

        return self.state

    def _generate_waypoints(self, start_long, start_lat, start_alt, num_points=12):
        """生成从起点到目标的优化航线，包含经纬度和高度"""
        if self.i < 2:
            end_long, end_lat, end_alt = TARGET_LONG, TARGET_LAT, TARGET_ALT
            num_points = 12  # 检查点数量（包含起点和终点）
            longitudes = np.linspace(start_long, end_long, num_points)  #
            latitudes = np.linspace(start_lat, end_lat, num_points)  #
            altitudes = np.linspace(start_alt, end_alt, num_points)  # 平滑爬升
            self.waypoints = list(zip(longitudes, latitudes, altitudes))
            logger.info(f"Generated waypoints: {self.waypoints}")
            logger.info(f"Generated {num_points} waypoints: {self.waypoints}")
            self.i = self.i + 1

    def _write_waypoints(self):
        """将中间检查点写入 waypoints.csv 文件"""
        csv_filename = "results//waypoints.csv"
        with open(csv_filename, 'w', newline='') as csvfile:
            writer = csv.writer(csvfile)
            writer.writerow(["Long", "Lat", "Alt"])  # 写入表头
            for waypoint in self.waypoints:
                writer.writerow([waypoint[0], waypoint[1], waypoint[2]])
        logger.info(f"Waypoints written to {csv_filename}")

    def _get_fallback_data(self):
        """当UDP数据不可用时返回模拟数据"""
        return {
            "M_": 0.5, "IAS": 150, "heading_deg": 0,
            "pitch_deg": 5, "bank_deg": 0,
            "Point": [0, 100, 0], "VV": [100, 10, 0],
            "AoA": 5, "ACC": [0, 0, 0],
            "Fuel": 0.8, "Gears": 0, "Flaps": 0,
            "AirBrake": 0, "RPM": [90, 90, 0, 0, 0, 0],
            "LLA_long": 45.019, "LLA_lat": 41.636, "LLA_balt": 200,
            "Omega": [0.0, 0.0, 0.0]  # 角速度
        }

    def step(self, action):
        # 激活DCS窗口
        activate_dcs_window()
        self.action = action  # 保存当前动作

        self._take_action(action)  # 执行动作

        # 控制频率动态调整

        # speed = data["IAS"]
        speed = self.state[1]  # 获取当前速度
        sleep_time = 0.2 if speed > 50 else 0.1
        time.sleep(max(sleep_time, 0))
        data = get_dcs_data()
        if data is None:
            logger.warning("Using fallback data in step")
            data = self._get_fallback_data()

        new_state = self._extract_state(data)  # 提取新状态
        current_distance = self._calculate_distance(data)  # 计算当前到目标航点的距离  二维距离
        current_altitude = new_state[32]  # 获取当前高度
        altitude_change = current_altitude - self.last_altitude  # 计算高度变化
        current_speed = new_state[1]  # 获取当前速度

        # self.flight_trajectory.append((data["LLA_long"], data["LLA_lat"]))

        # 记录飞行数据
        self._write_flight_data(data)

        # 更新飞行轨迹
        self.flight_trajectory.append((self.state[30], self.state[31], self.state[32]))

        reward = self._calculate_reward(new_state, current_distance, altitude_change)  # 计算奖励
        done = self._is_done(new_state, current_distance, current_altitude)  # 判断是否结束  返回值为Ture , 表示达到终止条件
        # if done:
        #     time.sleep(3)
        #     revive_control(duration=0.3)  # 飞机复活

        # 累积总奖励
        self.total_reward += reward

        # 将回合数、步数和奖励写入CSV
        self._write_to_csv(self.current_episode, self.steps, reward, self.total_reward, current_altitude, current_speed)

        self.state = new_state
        self.last_distance = current_distance
        self.last_altitude = current_altitude
        self.steps += 1

        logger.info(
            f"Alt={current_altitude:.1f}m | "
            f"Spd={self.state[1]:.1f}m/s | "
            f"Pitch={self.state[3]:.1f}° | "
            f"Reward={reward:.2f}"
        )

        # 记录飞行数据到FlightDataLogger.py
        self.data_logger.log_data(self.current_episode, self.steps, new_state)

        # 每隔一定步数或回合结束时保存数据
        if done or self.steps % 100 == 0:
            self.data_logger.save_to_csv()
            self.data_logger.plot_parameters()

        return new_state, reward, done, {}

    def _write_flight_data(self, data):
        """将飞行数据写入 CSV 文件"""
        # 计算到当前航点的距离
        if self.current_waypoint_idx < len(self.waypoints):
            target_long, target_lat, target_alt = self.waypoints[self.current_waypoint_idx]
            dist_to_waypoint = self._calculate_distance_to_point(data, target_long, target_lat)
        else:
            dist_to_waypoint = self._calculate_distance(data)

        # 组织飞行数据
        flight_record = {
            "initial_position": self.initial_position,
            "target_position": self.target_position,
            "current_position": (data["LLA_long"], data["LLA_lat"], data["LLA_balt"]),
            "current_speed": data["IAS"],
            "current_altitude": data["LLA_balt"],
            "distance_to_waypoint": dist_to_waypoint
        }

        # 写入 CSV 文件
        csv_filename = "results//flight_data.csv"
        import os
        file_exists = os.path.exists(csv_filename)

        with open(csv_filename, 'a', newline='') as csvfile:
            writer = csv.writer(csvfile)
            if not file_exists:
                # 写入表头
                writer.writerow([
                    "Initial Long", "Initial Lat", "Initial Alt",
                    "Target Long", "Target Lat", "Target Alt",
                    "Current Long", "Current Lat", "Current Alt",
                    "Current Speed", "Current Altitude", "Distance to Waypoint"
                ])
            # 写入数据
            writer.writerow([
                self.initial_position[0], self.initial_position[1], self.initial_position[2],
                self.target_position[0], self.target_position[1], self.target_position[2],
                data["LLA_long"], data["LLA_lat"], data["LLA_balt"],
                data["IAS"], data["LLA_balt"], dist_to_waypoint
            ])

        # 添加到飞行数据列表（用于绘图）
        self.flight_data.append(flight_record)

    def _write_to_csv(self, episode, step, reward, total_reward, ALT, IAS):
        """将回合数、步数、奖励和总奖励写入CSV文件"""
        try:
            with open(CSV_FILE_PATH, mode='a', newline='') as file:
                writer = csv.writer(file)
                writer.writerow([episode, step, reward, total_reward, ALT, IAS])
                logger.info(
                    f"Data written to CSV: Episode={episode}, Step={step}, Reward={reward}, Total Reward={total_reward}")
        except Exception as e:
            logger.error(f"Failed to write to CSV: {e}")

    def _extract_state(self, data):
        delta_long = math.radians(data["LLA_long"] - TARGET_LONG) * 6371000  # 计算经度偏差
        delta_lat = math.radians(data["LLA_lat"] - TARGET_LAT) * 6371000  # 计算纬度偏差
        delta_alt = data["LLA_balt"] - TARGET_ALT  # 计算高度偏差
        print("经度偏差:", delta_long, "纬度偏差:", delta_lat)

        # 计算目标航向角（从当前位置指向目标位置）
        lat1, lon1 = math.radians(data["LLA_lat"]), math.radians(data["LLA_long"])
        lat2, lon2 = math.radians(TARGET_LAT), math.radians(TARGET_LONG)
        dlon = lon2 - lon1
        y = math.sin(dlon) * math.cos(lat2)
        x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
        target_heading = math.degrees(math.atan2(y, x)) % 360  # 目标航向（0-360度）

        # 计算到参考路径的偏差
        if self.reference_path:
            path_deviation = self._calculate_path_deviation(data["LLA_long"], data["LLA_lat"])
            print("参考路径的偏差:", path_deviation)
        else:
            path_deviation = 0.0  # 如果路径未生成，默认偏差为0

        # 增加角速度和加速度信息，提取并返回状态向量
        '''
        状态空间中参数含义以及索引
            M_ : 马赫数  0   IAS: 指示空速 1  heading_deg:航向  2  pitch_deg:俯仰 3 bank_deg: 滚转 4  Point:位置   5 6 7
            VV:对地速度(三维)  8 9 10    AoA: 攻角 11  ACC:加速度 12 13 14     Fuel: 燃油 15   Gears:机轮  16
            Flaps: 襟翼  17   AirBrake:减速板  18    RPM:引擎 19  delta_long:经度偏差 20   delta_lat:纬度偏差  21   delta_alt: 高度偏差  22
            is_on_ground:地面标志位  23  Omega: 角速度 24 25 26    target_heading:目标航向  27   path_deviation: 路径偏差  28 
            --还需新增：
            VV_0:对地速度（一维）29  LLA_long：当前经度 30  LLA_lat：当前纬度 31 LLA_balt：当前高度 32

        '''
        state = np.array([
            data["M_"], data["IAS"], data["heading_deg"],
            data["pitch_deg"], data["bank_deg"],
            *data["Point"], *data["VV"], data["AoA"],
            *data["ACC"], data["Fuel"], data["Gears"],
            data["Flaps"], data["AirBrake"], data["RPM"][0],
            delta_long, delta_lat, delta_alt,
            1.0 if self.is_on_ground else 0.0,  # 添加标志位
            *data["Omega"],  # 角速度
            target_heading,  # 新增目标航向
            path_deviation,  # 新增路径偏差
            data["VV_0"], data["LLA_long"], data["LLA_lat"], data["LLA_balt"]
        ], dtype=np.float32)

        # 数据维度大小信息提示
        assert len(state) == self.observation_space.shape[
            0], f"State length {len(state)} does not match observation space {self.observation_space.shape[0]}"
        return state

    def _calculate_path_deviation(self, current_long, current_lat):
        # 计算到参考路径的偏差
        ref_points = np.array(self.reference_path)
        distances = np.sqrt((ref_points[:, 0] - current_long) ** 2 + (ref_points[:, 1] - current_lat) ** 2) * 6371000
        return np.min(distances)

    def _calculate_distance(self, data):
        lat1, lon1 = math.radians(data["LLA_lat"]), math.radians(data["LLA_long"])  # 将经纬度转换为弧度
        lat2, lon2 = math.radians(TARGET_LAT), math.radians(TARGET_LONG)
        return 6371000 * math.acos(
            math.sin(lat1) * math.sin(lat2) +
            math.cos(lat1) * math.cos(lat2) * math.cos(lon1 - lon2)
        )  # 使用球面距离公式计算距离

    def _take_action(self, action):
        '''
        throttle_action, pitch_action, roll_action, flaps_action, airbrake_action, foot_action
        油门              俯仰           滚转           襟翼           扰流板            脚舵
        '''
        global d_alt1
        '''
            执行飞机的控制动作，根据当前状态和输入的动作值控制飞机的油门、俯仰、滚转、襟翼、扰流板和脚舵。

            参数:
                action: 一个包含6个元素的数组，表示连续动作空间的控制量，范围为 [-1, 1]。
                        - throttle_action (action[0]): 油门控制，>0 增加油门，<0 减少油门
                        - pitch_action (action[1]): 俯仰控制，>0 机头向下（俯仰减小），<0 机头向上（俯仰增大）
                        - roll_action (action[2]): 滚转控制，>0 向右滚转，<0 向左滚转
                        - flaps_action (action[3]): 襟翼控制，>0 打开襟翼，<0 关闭襟翼
                        - airbrake_action (action[4]): 扰流板控制，>0 打开扰流板，<0 关闭扰流板
                        - foot_action (action[5]): 脚舵控制，>0 向左偏转，<0 向右偏转
            '''
        throttle_action, pitch_action, roll_action, flaps_action, airbrake_action, foot_action = action
        # 引入指数移动平均（EMA）平滑动作值，减少控制的突变
        # for i in range(6):
        #     self.smoothed_action[i] = 0.7 * self.last_action[i] + 0.3 * action[i]
        #     self.last_action[i] = self.smoothed_action[i]
        # throttle_action, pitch_action, roll_action, flaps_action, airbrake_action, foot_action = self.smoothed_action
        print("action", action)
        data = get_dcs_data()

        # 计算当前目标航点的航向和误差
        if not self.is_on_ground and self.current_waypoint_idx < len(self.waypoints):
            target_long, target_lat, target_alt = self.waypoints[self.current_waypoint_idx]
            lat1, lon1 = math.radians(data["LLA_lat"]), math.radians(data["LLA_long"])
            lat2, lon2 = math.radians(target_lat), math.radians(target_long)
            dlon = lon2 - lon1
            y = math.sin(dlon) * math.cos(lat2)
            x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
            target_heading = math.degrees(math.atan2(y, x)) % 360  # 计算目标航向
            # 计算航向偏差
            heading_error = math.degrees(math.atan2(math.sin(math.radians(target_heading - data["heading_deg"])),
                                                    math.cos(math.radians(target_heading - data["heading_deg"]))))
            if heading_error > 180:
                heading_error -= 360  # 将航向修正到[-180, 180]
            elif heading_error < -180:
                heading_error += 360
            print("当前航向误差为:", heading_error)
        else:
            target_heading = data["heading_deg"]  # 地面阶段或无航点时保持当前航向
            heading_error = 0
            self.heading_error = heading_error

        if self.is_on_ground:
            # 地面跑道阶段
            print("--" * 40)
            print("地面控制阶段：")
            print("目前飞机状态：高度", data["LLA_balt"], "速度：", data["IAS"], "襟翼：", data["Flaps"], "起落架：",
                  data["Gears"])
            min_takeoff_speed = MIN_TAKEOFF_SPEED  # 最小起飞速度   F-16C 起飞速度约 120 节 (62 m/s)，这里稍降低以优化  60 m/s == 216 km/h
            # 油门控制：最大化加速
            # if throttle_action > 0:
            #     throttle_up(duration=0.9 * abs(throttle_action))  # 增加油门响应时间，为了快速获得更大的速度  0.5
            # throttle_up(duration=1)
            # throttle_up(duration=0.9 * abs(throttle_action)) # 尽可能获取更大速度
            if data["LLA_balt"] - INIT_ALT < 50:
                throttle_action = np.clip(throttle_action, -1, 1)
                throttle_up(duration=2 * abs(throttle_action))  # 尽可能获取更大速度
                print("地面阶段油门加速时间为：", 2 * abs(throttle_action))

            # 脚舵校正跑道方向
            runway_heading = INIT_HEADING  # 假设跑道航向为 0°，可根据实际调整
            heading_error_ground = runway_heading - data["heading_deg"]
            # 增加条件：速度低于最小起飞速度时使用脚舵
            if data["IAS"] < min_takeoff_speed:
                print("地面跑道航向角偏转：", heading_error_ground)
                if abs(heading_error_ground) > 2:  # 航向角偏转大小
                    # foot_control = np.clip(heading_error_ground * 0.1, -1, 1)
                    foot_control = foot_action

                    if foot_control > 0:
                        foot_left(duration=0.2)  # 向左调整
                    else:
                        foot_right(duration=0.2)  # 向右调整

            # 襟翼控制：尽早打开以增加升力
            '''
                 如果速度大于20 m/s且襟翼位置大于0.5（表示襟翼处于着陆襟翼0.9），目标为展开襟翼到0.25  
                 由于0.9-->0.5的过程会持续一段时间，有时会执行两次关闭襟翼动作，导致襟翼为0而不是所需要的0.25，,故将阈值改为0.8，
            '''
            # if data["IAS"] > 0 and data["Flaps"] > 0.8:  # 降低速度阈值至 5 m/s == 18 km/h  为了快速加速。处于着陆襟翼时加速较慢
            #     flaps_control(duration=0.2)

            # 俯仰控制：达到最小起飞速度立即拉起
            print("襟翼：", data["Flaps"])
            if data["IAS"] > min_takeoff_speed:
                # pitch_control = np.clip(pitch_action, -1, 1)
                pitch_control = pitch_action
                if data["pitch_deg"] < 5:
                    print("当前俯仰角小于5度，具体为：", data["pitch_deg"])
                    pitch_down(duration=1.5 * abs(pitch_control))  # 操作杆后拉
                    print("地面阶段在俯仰角小于5度时将操作杆后拉持续时间为：", 1.5 * abs(pitch_control))
                    # time.sleep(2.5)  # 状态延迟两秒
                    pitch_up(duration=1.5 * abs(pitch_control))  # 前推


                elif data["pitch_deg"] < 12:
                    print("当前俯仰角位于[5,10)度，具体为：", data["pitch_deg"])
                    pitch_down(duration=0.3 * abs(pitch_control))  # 操作杆后拉
                    print("地面阶段在俯仰角位于[5,10)度时将操作杆后拉持续时间为：", 0.3 * abs(pitch_control))
                    # time.sleep(2)
                    pitch_up(duration=0.3 * abs(pitch_control))
                else:
                    print("当前俯仰角大于12度，具体为：", data["pitch_deg"])
                    pitch_up(duration=0.3 * abs(pitch_control))  # 操作杆前推
                    print("地面阶段在俯仰角大于12度时将操作杆前推持续时间为：", 0.5 * abs(pitch_control))
                    # time.sleep(2)
                    pitch_down(duration=0.3 * abs(pitch_control))
            if data["LLA_balt"] - INIT_ALT > 25:  # 25 米收起落架
                gearsoff_control(duration=0.2)  # 收起起落架

                # if pitch_control > 0:
                #     if 20 < data["pitch_deg"] < 45:
                #         print("当前俯仰角位于[20,45)度，具体为：", data["pitch_deg"])
                #         pitch_down(duration=5 * abs(pitch_control))
                #     if data["pitch_deg"] > 45 :
                #         print("当前俯仰角大于45度，具体为：", data["pitch_deg"])
                #         pitch_up(duration=5 * abs(pitch_control))

            # 滚转控制：RL 直接输出，负责增稳
            # if data["IAS"] > min_takeoff_speed - 10 :

            if data["LLA_balt"] - INIT_ALT > 100:
                roll_control = np.clip(roll_action, -1, 1)
                print("当前飞机滚转度为：", data["bank_deg"])
                print("roll_action:", roll_action)
                print("roll_control:", roll_control)
                if data["bank_deg"] > 0:
                    # if roll_action > 0:
                    # roll_right(duration=0.2 * abs(roll_control))
                    roll_left(duration=0.2)
                else:
                    # roll_left(duration=0.2 * abs(roll_control))
                    roll_right(duration=0.2)

            if (data["LLA_balt"] - INIT_ALT) > 50 and data["RPM"][0] > 0.95:
                throttle_down(duration=0.3)  # 油门减少

            # 切换到空中模式
            if data["LLA_balt"] - INIT_ALT > 122:  # 飞机高度比机场高度大于50米时，认为在空中
                self.is_on_ground = False
                logger.info("Aircraft has taken off, switching to air mode")
        else:
            print("空中控制阶段：")
            print("目前飞机状态：高度", data["LLA_balt"], "速度：", data["IAS"], "襟翼：", data["Flaps"], "起落架：",
                  data["Gears"])

            # 引入指数移动平均（EMA）平滑动作值，减少控制的突变
            # for i in range(6):
            #     self.smoothed_action[i] = 0.7 * self.last_action[i] + 0.3 * action[i]
            #     self.last_action[i] = self.smoothed_action[i]
            # throttle_action, pitch_action, roll_action, flaps_action, airbrake_action, foot_action = self.smoothed_action
            # print("self.smoothed_action", self.smoothed_action)

            # 空中阶段：优化速度、高度和路径
            d_alt = TARGET_ALT - data["LLA_balt"]  # 高度偏差
            d_v = TARGET_VA - data["IAS"]  # 速度偏差

            print("高度偏差：", d_alt, "速度偏差：", d_v, "航向偏差:", heading_error)

            print("--" * 40)

            # 检查是否到达当前航点（仅在空中阶段且有航点时执行）
            if self.current_waypoint_idx < len(self.waypoints):  # 当前检查点序号+1
                target_long, target_lat, target_alt = self.waypoints[self.current_waypoint_idx]
                dist_to_waypoint = self._calculate_distance_to_point(data, target_long, target_lat)
                # if dist_to_waypoint < 500 and abs(d_alt) < 100:  # 距离 < 500m 且高度偏差 < 100m 满足此条件进行航点切换
                #     self.current_waypoint_idx += 1
                print("当前目标是第", self.current_waypoint_idx, "个检查点")
                # 引入航向判断 确保飞机在接近航点时航向已对准下一个航点，避免偏离路径
                # heading_to_waypoint = math.degrees(
                #     math.atan2(target_lat - data["LLA_lat"], target_long - data["LLA_long"])) % 360
                # heading_error = min(abs(heading_to_waypoint - data["heading_deg"]),
                #                     360 - abs(heading_to_waypoint - data["heading_deg"]))

                d_alt1 = target_alt - data["LLA_balt"]
                dist_to_waypoint_3d = np.sqrt(dist_to_waypoint ** 2 + d_alt1 ** 2)
                print("距离当前检查点的三维距离为：", dist_to_waypoint_3d)

                if dist_to_waypoint_3d < 4000:  # 三维距离小于500 高度小于300

                    self.current_waypoint_idx += 1

                    # self.last_distance = 0
                    print("已将检查点从第", self.current_waypoint_idx - 1, "个检查点切换到第",
                          self.current_waypoint_idx, "个检查点")
                    # 切换后立即调整航向和俯仰  在航点切换后，立即调整飞机的航向和俯仰角以对准下一个航点
                    if self.current_waypoint_idx < len(self.waypoints):

                        next_long, next_lat, next_alt = self.waypoints[self.current_waypoint_idx]
                        print("下一个点的三维坐标为: 经度：", next_long, "纬度:", next_lat, "高度", next_alt)

                        d_alt2 = next_alt - data["LLA_balt"]
                        dist_to_waypoint = self._calculate_distance_to_point(data, next_long, next_lat)
                        dist_to_waypoint_3d_1 = np.sqrt(dist_to_waypoint ** 2 + d_alt2 ** 2)
                        print("距离下一个检查点的三维距离为：", dist_to_waypoint_3d_1)
                        self.last_distance = dist_to_waypoint_3d_1

                        heading_to_next = math.degrees(
                            math.atan2(next_lat - data["LLA_lat"], next_long - data["LLA_long"])) % 360
                        # heading_error = heading_to_next - data["heading_deg"] 航向角计算有误
                        lat1, lon1 = math.radians(data["LLA_lat"]), math.radians(data["LLA_long"])
                        lat2, lon2 = math.radians(next_lat), math.radians(next_long)
                        dlon = lon2 - lon1
                        y = math.sin(dlon) * math.cos(lat2)
                        x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
                        next_heading = math.degrees(math.atan2(y, x)) % 360  # 计算目标航向
                        # 计算航向偏差
                        heading_error = math.degrees(
                            math.atan2(math.sin(math.radians(next_heading - data["heading_deg"])),
                                       math.cos(math.radians(next_heading - data["heading_deg"]))))
                        if heading_error > 180:
                            heading_error -= 360  # 将航向修正到[-180, 180]
                        elif heading_error < -180:
                            heading_error += 360

                        pitch_adjust = np.clip((next_alt - data["LLA_balt"]) / 500, -1, 1)
                        print("下一个点的航向为：", heading_to_next)
                        print("下一个点的航向误差为：", heading_error)
                        print("下一个点产生的俯仰动作调整为：", pitch_adjust)
                        if pitch_adjust > 0:
                            pitch_down(duration=0.1 * abs(pitch_action))  # 调整俯仰
                            print("下一个点进行操作杆后拉持续时间为：", 0.1 * abs(pitch_action))
                            # time.sleep(0.5)
                            pitch_up(duration=0.1 * abs(pitch_action))
                        else:
                            pitch_up(duration=0.2 * abs(pitch_action))
                            print("下一个点进行操作杆前推持续时间为：", 0.2 * abs(pitch_action))
                            # time.sleep(0.5)
                            pitch_down(duration=0.2 * abs(pitch_action))

                        # 目前检查点切换时的滚转存在问题
                        roll_target = np.clip(heading_error * 0.2, -5, 5)
                        print("下一个点产生的滚转动作调整为：", roll_target)
                        if roll_target > 0:
                            roll_right(duration=abs(roll_action))  # 调整滚转
                            print("下一个点向右滚转持续时间为：", abs(roll_action))
                        else:
                            roll_left(duration=abs(roll_action))
                            print("下一个点向左滚转持续时间为：", abs(roll_action))
                    logger.info(f"Reached waypoint {self.current_waypoint_idx - 1}, moving to next")

            if data["RPM"][0] > 0.95:
                throttle_down(duration=0.1)
            # 油门控制：加速至目标速度
            if data["RPM"][0] > 0.97:
                throttle_down(duration=0.2)

            print("--" * 40)
            if d_v > 30:  # 小于目标速度
                throttle_up(duration=0.5 * abs(throttle_action))  # 油门增加
                print("小于目标速度油门持续时间为：", 0.5 * throttle_action)
                if data["RPM"][0] < 0.85:  # 如果引擎小于0.75 ， 油门增加
                    print(("小于目标速度，引擎<0.85,真实数值为：", data["RPM"][0]))
                    throttle_up(duration=0.2)
            elif d_v < -30:  # 大于目标速度
                throttle_down(duration=0.2 * abs(throttle_action))  # 油门减少
                print("大于目标速度油门持续时间为：", 0.3 * abs(throttle_action))
                if data["RPM"][0] < 0.85:
                    print(("大于目标速度，引擎<0.85,真实数值为：", data["RPM"][0]))
                    throttle_up(duration=0.2)

            # 俯仰控制：快速调整高度
            '''
                降低高度偏差阈值：将高度控制的触发阈值从100米调整为50米，提升精度。
                引入垂直速度反馈：结合垂直速度动态调整俯仰角，使高度控制更平滑。
            '''
            print("--" * 40)
            # d_alt1 = target_alt - data["LLA_balt"]
            target_vspeed = np.clip(d_alt1 / 5, -5, 5)  # 目标垂直速度   计算目标垂直速度，限制在 [-5, 5] m/s
            print("d_alt1/5", d_alt1 / 5)
            print("目标垂直速度 target_vspeed", target_vspeed)
            vspeed_error = target_vspeed - data["VV"][2]  # 垂直速度偏差
            print("垂直速度偏差 vspeed_error", vspeed_error)
            pitch_adjust = np.clip(vspeed_error * 0.1, -1, 1)  # 根据垂直速度偏差调整俯仰
            print("根据垂直速度偏差调整俯仰 pitch_adjust", pitch_adjust)
            print("强化学习产生的俯仰动作值 pitch_action", pitch_action)

            target_long, target_lat, target_alt = self.waypoints[self.current_waypoint_idx]
            print("当前检查点的高度", target_alt)
            d_alt_waypoints = target_alt - data["LLA_balt"]
            if d_alt_waypoints > 300:  # 高度低于目标300米以上， 需要操纵杆后拉，使机头朝上
                # pitch_down(duration=0.1 * pitch_adjust)
                pitch_down(duration=0.1 * abs(pitch_action))
                print("操纵杆后拉加权时间：", 0.1 * abs(pitch_action))
                # time.sleep(0.5)
                pitch_up(duration=0.1 * abs(pitch_action))

            elif d_alt_waypoints < -200:  # 高度高于目标300米，需要操纵杆前推，使机头朝下
                # pitch_up(duration=0.1 * abs(pitch_adjust))
                pitch_up(duration=0.1 * abs(pitch_action))
                print("操纵杆前推加权时间：", 0.1 * abs(pitch_action))
                # time.sleep(0.5)
                pitch_down(duration=0.1 * abs(pitch_action))
                # pitch_down(duration=0.1)

            if data["pitch_deg"] > 15:
                pitch_up(duration=0.01)
                # time.sleep(1)
                pitch_down(duration=0.01)

            if data["pitch_deg"] < -5:
                pitch_down(duration=0.02)
                # time.sleep(1)
                pitch_up(duration=0.02)

            # if d_alt > 100 and pitch_action > 0:  # 低于目标高度，爬升
            #     pitch_down(duration=0.3 * pitch_action)
            # elif d_alt < -100 and pitch_action < 0:  # 高于目标高度，下降
            #     pitch_up(duration=0.3 * abs(pitch_action))

            # if abs(pitch_action) > 0.1:  # 降低阈值，增加俯仰控制频率
            #     pitch_down(duration=0.3 * pitch_action) if pitch_action > 0 else pitch_up(duration=0.3 * abs(pitch_action))

            #  滚转控制：调整航向至目标航点
            print("--" * 40)
            roll_target = np.clip(heading_error * 0.5, -5, 5)  # 限制滚转角度
            print("航向角误差heading_error", heading_error)
            print("加权后的目标滚转角度roll_target", roll_target)
            print("强化学习产生的滚转动作roll_action", roll_action)

            if roll_target > 0:
                roll_right(duration=abs(roll_action))  # 向右滚转
                # roll_right(duration=0.3)   # 在不使用roll_action可以获得更大滚转幅度
                print("向右滚转持续时间为：", abs(roll_action))
            elif roll_target < 0:
                roll_left(duration=abs(roll_action))  # 向左滚转
                # roll_left(duration=0.3)
                print("向左滚转持续时间为：", abs(roll_action))

            # if abs(heading_error) < 5:
            #     if heading_error > 0:
            #         roll_right(duration=0.1)
            #     else:
            #         roll_left(duration=0.1)
            # else:
            #     if heading_error > 0:
            #         roll_right(duration=0.2)
            #     else:
            #         roll_left(duration=0.2)

            if abs(data["bank_deg"]) > 3:  # 3   15
                if data["bank_deg"] > 0:
                    roll_left(duration=0.1)  # 向左滚转
                else:
                    roll_right(duration=0.1)  # 向右滚转

            # 襟翼控制：增加升力以快速爬升  F-16C襟翼为飞行系统自动控制
            '''
            if d_alt > 500 and d_v < -1 and flaps_action > 0:  # 高度偏差大且速度大于目标速度，打开襟翼，便于快速爬升
                flaps_control(duration=0.2 * flaps_action)     # 打开襟翼
            elif d_alt < 50 and flaps_action < 0:   # 接近目标高度，关闭襟翼
                flaps_control(duration=0.2 * abs(flaps_action))  # 关闭襟翼
            '''

            # 扰流板控制：减速以调整高度或速度
            print("--" * 40)
            if d_v < -10 and data["AirBrake"] < 0.1:  # 扰流板的值<0
                print("大于目标速度时，强化学习产生的扰流板动作airbrake_action：", airbrake_action)
                airbrake_on_control(duration=0.2)  # 速度太高 ， 打开扰流板
                print("打开扰流板操作时间为：", 0.2 * abs(airbrake_action))
            elif d_v > 10 and data["AirBrake"] > 0.1:  # 扰流板的值>0
                print("小于目标速度时，强化学习产生的扰流板动作airbrake_action：", airbrake_action)
                airbrake_off_control(duration=0.2 * abs(airbrake_action))  # 速度过低 ，关闭扰流板
                print("关闭扰流板操作时间为：", 0.2 * airbrake_action)

            # 脚舵控制：精细调整航向
            print("--" * 40)
            foot_control = np.clip(heading_error * 0.1, -1, 1)
            print("航向角误差heading_error", heading_error)
            print("加权后的航向角误差：", heading_error * 0.1)
            print("裁剪后后脚舵控制：", foot_control)
            print("强化学习产生的脚舵动作foot_action", foot_action)
            if foot_control > 0:
                # foot_left(duration=0.2)
                foot_left(duration=abs(foot_action))
                print("脚舵向左控制时间为：", abs(foot_action))
            else:
                # foot_right(duration=0.2 * abs(foot_control))
                foot_right(duration=abs(foot_action))
                print("脚舵向右控制时间为：", abs(foot_action))

        # time.sleep(0.5) # 等待操作完成
        data = get_dcs_data()  # 获取数据
        if data is None:
            logger.warning("No feedback data after action execution")
        else:
            logger.debug(f"Action feedback - Altitude: {data['LLA_balt']:.1f}m, Speed: {data['IAS']:.1f}m/s")

    def _calculate_distance_to_point(self, data, target_long, target_lat):  # 二维坐标两点之间水平距离
        """计算到指定点的距离"""
        lat1, lon1 = math.radians(data["LLA_lat"]), math.radians(data["LLA_long"])
        lat2, lon2 = math.radians(target_lat), math.radians(target_long)
        return 6371000 * math.acos(
            math.sin(lat1) * math.sin(lat2) +
            math.cos(lat1) * math.cos(lat2) * math.cos(lon1 - lon2)
        )

    def _calculate_reward(self, state, current_distance, altitude_change, altitude_change_reward=None):
        '''
            计算当前步骤的奖励值，根据飞机的状态和任务目标设计奖励函数。

            参数:
                state: 当前状态向量，包含飞机的速度、姿态、位置等信息（33维）
                current_distance: 当前到目标航点的距离（米）
                altitude_change: 当前高度变化（米）

            返回:
                reward: 当前步骤的奖励值，综合考虑速度、高度、距离、稳定性等因素
            '''
        reward = 0
        heading_reward = 0  # 航向奖励
        alt_reward = 0  # 高度奖励
        alt_error_reward = 0  # 高度偏差惩罚

        delta_alt = state[22]  # 高度偏差
        current_altitude = delta_alt + TARGET_ALT  # 当前绝对高度
        speed = state[1]  # 指示空速
        pitch = abs(state[3])  # 俯仰角绝对值
        bank = abs(state[4])  # 滚转角绝对值
        heading = state[2]  # 当前航向
        target_heading = state[27]  # 目标航向
        path_deviation = state[28]  # 路径偏差
        aoa = state[11]  # 攻角
        vspeed = state[10]  # 垂直速度
        ground_speed = state[29]  # 对地速度

        pitch_rate = state[25]  # Omega[1] 俯仰角速度
        roll_rate = state[24]  # Omega[0] 滚转角速度

        # 动态权重
        if self.is_on_ground:
            w_speed = 0.2  # 提高速度权重
            w_altitude = 0.25
            w_distance = 0.0
            w_stability = 0.5  # 稳定性
            w_heading = 0.0  # 地面阶段轻微关注航向
            w_vspeed = 0.05  # 新增垂直速度权重

        else:
            w_speed = 0.1
            w_altitude = 0.15
            w_distance = 0.25  # 提高距离权重
            w_stability = 0.2
            w_heading = 0.2
            w_vspeed = 0.05  # 新增垂直速度权重

        # 地面阶段奖励（任务 2 优化）
        if self.is_on_ground:
            print("--" * 40)
            # 1. 速度奖励：激励快速加速
            speed_reward = 0
            min_takeoff_speed = 60  # 最小起飞速度
            if speed > min_takeoff_speed or speed == min_takeoff_speed:
                speed_reward += 50
            else:
                speed_reward -= 100

            print("地面阶段速度奖励：", speed_reward)

            # 2. 起飞奖励：成功离地
            altitude_change_reward = 0
            if altitude_change > 30:  # 高度增加 > 30
                altitude_change_reward += 50
            elif altitude_change < 0:
                altitude_change_reward -= 100

            print("地面阶段高度持续增加奖励：", altitude_change_reward)

            # 3. 稳定性奖励
            stability_reward = 0
            # 俯仰角
            if 5 < pitch < 15:
                stability_reward += 50
            else:
                stability_reward -= 100
            # 滚转角
            if abs(bank) < 5:
                stability_reward += 50
            elif abs(bank) < 15:
                stability_reward += 30
            elif abs(bank) < 30:
                stability_reward += 10
            else:
                stability_reward -= 100
            # 攻角
            if 11 < aoa < 13:  # 合理攻角范围
                stability_reward += 100
            else:
                stability_reward -= 200

            print("地面阶段稳定性奖励：", stability_reward)

            # 4. 垂直速度奖励
            vspeed_reward = 0

            if vspeed > 0:
                vspeed_reward += 50  # 激励离地
            else:
                vspeed_reward -= 100

            print("地面阶段垂直速度奖励：", vspeed_reward)

            # 5. 时间惩罚：激励最快起飞
            time_reward = 0
            time_reward -= self.steps * 10.0  # 每步惩罚 10.0，强迫快速离地
            print("地面阶段时间惩罚：", time_reward)

            # 6. 加权总奖励
            reward = w_speed * speed_reward + w_altitude * altitude_change_reward + w_stability * stability_reward + w_vspeed * vspeed_reward + time_reward
            print("地面阶段加权奖励：", reward)

        # 空中阶段奖励（任务 1 改进：跟踪航线）
        if not self.is_on_ground:
            print("--" * 40)
            target_speed = TARGET_VA  # 目标速度
            '''
            speed_error = abs(speed - target_speed)  # 实际速度与目标速度的差值绝对值
            speed_reward = 0
            # speed_reward = 50 * (1 - speed_error / target_speed) if speed_error < target_speed else -speed_error * 0.5
            # speed_reward = 20 * (1 - speed_error / target_speed) if speed_error < 10 else -speed_error * 10  # 接近目标速度奖励，否则惩罚
            if speed_error < 5:  # 0 < speed_error <5
                speed_reward += 50
            elif speed_error < 10:  # 5 < speed_error <10
                speed_reward += 30
            else:
                speed_reward -= 100
            print("空中阶段速度奖励为：",speed_reward)
            reward += w_speed * speed_reward
            '''
            speed_error = abs(speed - target_speed)  # 与当前检查点的速度差
            speed_reward = 50 * np.exp(-(speed_error / 10) ** 2)  # 平滑奖励  speed_error 越小，奖励越大
            # if hasattr(self, 'last_ground_speed'):
            if self.last_ground_speed is not None:  # 检查 last_ground_speed 是否有效
                speed_change = abs(ground_speed - self.last_ground_speed)
                speed_reward -= 20 * speed_change  # 惩罚快速变化
            self.last_ground_speed = ground_speed
            reward += w_speed * speed_reward
            print("空中阶段速度奖励为：", speed_reward)

            '''
            # 当前航点的距离和高度奖励    （仅限空中）
            if self.current_waypoint_idx < len(self.waypoints):
                target_long, target_lat, target_alt = self.waypoints[self.current_waypoint_idx]  # 获取当前航点的经纬度以及高度
                dist_to_waypoint = self._calculate_distance_to_point(
                    {"LLA_lat": state[31], "LLA_long": state[30]}, target_lat, target_long)  # 到当前航点距离
                alt_error = abs(target_alt - current_altitude)  # 高度差绝对值

                # 距离奖励 ：接近航点和距离减少
                if self.last_distance is not None:
                    distance_reduction = self.last_distance - dist_to_waypoint  # 二维上面距离减少量  上一次距离 - 这一次距离
                    dist_reward = 0
                    # distance_reduction =  path_deviation
                    if distance_reduction > 0 :  # 减少的距离 > 0
                        dist_reward += 200
                    else:
                        dist_reward -= 100
                    print("距离减少奖励：",dist_reward)

                    # dist_reward = distance_reduction * 200  # 每米距离进行奖励
                    # if dist_to_waypoint < 500:  # 接近航点进行奖励
                    #     dist_reward += 10 * (1 - dist_to_waypoint / 500)  # 接近航点大幅奖励

                    # 到达航点进行奖励
                    if dist_to_waypoint < 500 and abs(delta_alt) < 300:
                        reward += 500  # 到达航点奖励
                        print("获得到达航点奖励")
                    reward += w_distance * dist_reward
                    print("路径偏差惩罚：",state[28]/100)
                    reward -= w_distance * state[28] / 100  # 路径偏差惩罚，每100米减分
                self.last_distance = dist_to_waypoint  # 更新 last_distance 为当前航点距离
                '''
            # 2. 距离和高度奖励
            if self.current_waypoint_idx < len(self.waypoints):
                target_long, target_lat, target_alt = self.waypoints[self.current_waypoint_idx]  # 获取当前航点的经纬度以及高度
                dist_to_waypoint_2d = self._calculate_distance_to_point(
                    {"LLA_lat": state[31], "LLA_long": state[30]}, target_lat, target_long)  # 到当前航点距离  二维
                alt_error = abs(target_alt - current_altitude)  # 高度差绝对值
                dist_to_waypoint_3d = np.sqrt(dist_to_waypoint_2d ** 2 + alt_error ** 2)  # 三维距离

                # 距离奖励
                dist_reward = 0
                if self.last_distance is not None:
                    distance_reduction = self.last_distance - dist_to_waypoint_3d
                    dist_reward = 2 * distance_reduction  # 奖励三维距离减少 200改成2  20  2
                    if dist_to_waypoint_3d < 4000:
                        dist_reward += 1000  # 接近航点的额外奖励
                        print("获得到达检查点奖励")
                        # self.current_waypoint_idx += 1  # 切换到下一个检查点
                self.last_distance = dist_to_waypoint_3d
                reward += w_distance * dist_reward
                print("距离减少奖励：", dist_reward)

                alt_reward = 50 * np.exp(-(alt_error / 100) ** 2)  # 平滑高度奖励
                alt_reward += -100 * max(0, current_altitude - (target_alt + 500))  # 实时高度比目标高度高于500米时进行惩罚
                reward += w_altitude * alt_reward
                print("高度奖励为：", alt_reward)

                '''
                # 高度奖励
                if alt_error < 300:
                    alt_reward += alt_error * (1 - alt_error / 300)
                elif alt_error < 500:
                    alt_reward += (500 - alt_error) * (1 - alt_error / 500)
                else:
                    alt_reward -= 100 * (alt_error / 500)
                print("与当前航点的高度奖励为：",alt_reward)
                reward += w_altitude * alt_reward       
                '''
                # # 高度奖励
                # alt_reward = 10 * (1 - (alt_error / 500) ** 2) if alt_error < 500 else -alt_error * 10  # 与目标高度距离小于500，进行奖励，否则进行惩罚
                # alt_error_reward = -20 * (1 - (alt_error / 500) ** 2) if abs(alt_error) > 1000 else abs(alt_error) * 10  # 高度偏差惩罚
                '''
                如果alt_error > 1000（高度偏差极大）：
                惩罚公式：-20 * (1 - (alt_error / 500) ** 2)
                含义：
                当偏差远大于500米时，(alt_error / 500) ^ 2的值很大，导致(1 - x) 为负。最终惩罚为 - 20 * (负值) = 正惩罚（例如，偏差 1000米时惩罚为 - 20 * (1 - 4) = 60）。
                惩罚随偏差呈二次增长，比线性更严厉。
                目的：针对极端高度偏差（如俯冲或爬升过快）施加额外惩罚。
                如果 alt_error ≤ 1000：
                惩罚公式：abs(alt_error) * 10
                含义：每米偏差惩罚 10 点（与 alt_reward 的惩罚分支一致）。
                作用：保持对小偏差的线性惩罚，避免过度奖励。
                '''
                # reward += w_altitude * alt_reward
                # reward += w_altitude * (alt_reward + alt_error_reward)
                # reward += alt_reward + alt_error_reward

                # 航向奖励
                # heading_error = min(abs(heading - target_heading), 360 - abs(heading - target_heading)) # 航向偏差
                '''
                heading_error = self.heading_error
                print("飞机与当前航点的航向偏差为：",heading_error)
                if heading_error < 5:  # 航向偏差小于10进行奖励，否则进行惩罚
                    # heading_reward += w_heading * 20 * (1 - heading_error / 5)
                    heading_reward += 50 * (1 - heading_error / 5)
                elif heading_error < 10:
                    heading_reward += 30 * (1 - heading_error / 10)
                else:
                    # heading_reward -= w_heading * heading_error * 50  #  -= 惩罚
                    heading_reward -= 100 * (heading_error / 10)
                print("飞机与当前航点的航向偏差的奖励为：", heading_reward)
                reward += w_heading * heading_reward
                '''

                # 3. 航向奖励
                heading_error = abs(self.heading_error)  # 从 _take_action 中获取
                heading_reward = 50 * np.exp(-(heading_error / 10) ** 2)
                # if hasattr(self, 'last_heading'):
                if self.last_heading is not None:
                    heading_change = abs(heading - self.last_heading)
                    heading_reward -= 10 * heading_change  # 惩罚快速转弯
                self.last_heading = heading
                reward += w_heading * heading_reward
                print("航向奖励为：", heading_reward)

                '''
                # 垂直速度奖励/惩罚
                vspeed_reward = 0
                if abs(vspeed) < 5:
                    # reward += w_vspeed * 20  # 保持平稳爬升或下降
                    vspeed_reward += 30 * (1 - abs(vspeed) / 5)
                elif abs(vspeed) < 10:
                    vspeed_reward += 20 * (1 - abs(vspeed) / 10)
                else:
                    vspeed_reward -= 50 * (abs(vspeed) / 10)
                    # reward -= w_vspeed * abs(vspeed) * 30  # 惩罚过大垂直速度
                print("对当前航点飞机的垂直速度奖励为：",vspeed_reward)
                reward += w_vspeed * vspeed_reward


                # 攻角稳定性  攻角对于飞机的升力有很重要作用
                aoa_reward = 0
                if 0 < aoa < 15:
                    aoa_reward += 50 * aoa
                # elif aoa < 25:
                #     aoa_reward += 30
                else:
                    aoa_reward -= 100 * abs(aoa)
                print("对当前航点飞机额度攻角稳定性奖励为：",aoa_reward)
                reward += w_stability * aoa_reward
                # if 5 < aoa < 10:
                #     # reward += w_stability * 50
                #     stability_reward1 += w_stability * 50
                #     reward += stability_reward1
                # elif aoa < 0 or aoa > 15:
                #     # reward -= w_stability * abs(aoa - 10) * 20
                #     stability_reward1 -= w_stability * abs(aoa - 10) * 20
                #     reward -= stability_reward1
                '''

                # 4. 垂直速度和攻角奖励
                vspeed_reward = 30 * np.exp(-(abs(vspeed) / 5) ** 2)  # 偏好小的垂直速度
                reward += w_vspeed * vspeed_reward
                print("垂直速度奖励为：", vspeed_reward)

                aoa_reward = 50 if 11 < aoa < 13 else -100 * abs(aoa - 12)  # 攻角范围 11-13°
                reward += w_stability * aoa_reward
                print("攻角稳定性奖励为：", aoa_reward)

            # 稳定性惩罚
            '''
            stability_reward = 0  # 稳定性奖励
            pitch_stability_reward = 0 # 俯仰稳定性奖励
            bank_stability_reward = 0  # 滚转稳定性奖励
            if -5 < pitch < 15:
                pitch_stability_reward += 50 * abs(pitch)
            # elif pitch < 35:
            #     pitch_stability_reward += 30
            else:
                pitch_stability_reward -= 100 * abs(pitch)
            print("空中阶段俯仰角稳定性奖励为：",pitch_stability_reward)

            if abs(bank) < 10 :
                bank_stability_reward += 50 * (1 - abs(bank) / 10)
            elif abs(bank) < 25:
                bank_stability_reward +=10 * (1 - abs(bank) / 25)
            else:
                bank_stability_reward -= 100
            print("空中阶段滚转角稳定性奖励为：",bank_stability_reward)

            stability_reward = pitch_stability_reward + bank_stability_reward

            print("空中阶段稳定性总奖励为：",stability_reward)

            reward += stability_reward * w_stability
            '''

            # 5. 稳定性奖励
            stability_reward = 0
            roll_rate = abs(state[24])  # 滚转速率
            pitch_rate = abs(state[25])  # 俯仰速率
            pitch = abs(pitch)
            if pitch < 10:
                stability_reward += 50
            elif pitch < 20:
                stability_reward += 20
            else:
                stability_reward -= 100 * (pitch / 20)
            bank = abs(bank)
            if bank < 10:
                stability_reward += 50
            elif bank < 20:
                stability_reward += 20
            else:
                stability_reward -= 100 * (bank / 20)
            stability_reward -= 50 * (roll_rate + pitch_rate) * 100  # 惩罚快速旋转
            reward += w_stability * stability_reward
            print("空中阶段稳定性奖励为：", stability_reward)

            # if pitch > 15:  # 俯仰角
            #     stability_penalty += (pitch - 10) * 50
            # if pitch < -10:
            #     stability_penalty += (abs(pitch) - 10) * 50
            # if abs(bank) > 15:
            #     stability_penalty += (bank - 15) * 50
            # reward -= w_stability * stability_penalty
            #
            # if abs(pitch) < 10:
            #     stability_reward += 20
            # if abs(bank) < 10:
            #     stability_reward += 20
            # # reward += w_stability * stability_reward
            # reward += stability_reward

            # 6. 动作惩罚
            action_penalty = -20 * np.sum(np.abs(self.action))  # 惩罚大幅动作
            # if hasattr(self, 'last_action_stored'):
            if self.last_action_stored is not None:
                action_change = np.sum(np.abs(self.action - self.last_action_stored))
                action_penalty -= 10 * action_change  # 惩罚快速变化
            self.last_action_stored = self.action.copy()
            reward += action_penalty
            print("空中阶段动作惩罚：", action_penalty)

            # reward -= 10.0  # 时间惩罚
            # 8. 坠机预防
            if speed < 50 or current_altitude < 100 or pitch > 30 or bank > 30:
                reward -= 1000  # 提前惩罚危险状态
                print("危险状态惩罚！")
            # 时间惩罚
            time_reward = 0
            time_reward -= self.steps * 10.0  # 每步惩罚 10.0，强迫快速到达目标点
            reward += time_reward
            print("空中阶段执行动作的总奖励为：", reward)

        # 坠毁惩罚
        if speed < 1:
            reward -= 2000
            print("飞机坠毁进行惩罚！")

        self.last_pitch = pitch
        self.last_bank = bank
        self.total_reward += reward

        # 记录飞行数据到FlightDataLogger.py

        return reward

    def _is_done(self, state, current_distance, current_altitude):
        # 新增安全监测条件
        # if current_altitude < 200:
        #     # logger.warning("CRASH DETECTED!")
        #     logger.warning("飞机坠毁!")
        #     return True
        # if abs(state[3]) > 40 or abs(state[4]) > 40:  # 更大姿态容限  俯仰  滚转
        #     logger.warning("危险姿态!")
        #     return True
        if state[1] < 1:
            logger.info("飞机已坠毁！")
            return True
        # if state[15] <= 0.05:
        #     logger.warning("Fuel almost exhausted")
        #     return True
        # if self.steps >= 100:
        #     logger.warning("Training time limit reached")
        #     return True
        # if not self.is_on_ground and current_distance < 500:
        #     logger.info("Target reached!")
        #     self._plot_trajectory()  # 到达目标时绘制轨迹
        #     return True
        # if self.total_reward > 1000000:
        #     logger.info("奖励已达到最大！")
        #     return  True
        # if self.total_reward < -1000000:
        #     logger.info("奖励已达到最小！")
        #     return  True
        # if not self.is_on_ground and state[28] > 10000:  # 路径偏差超过 8km
        #     logger.warning("路径偏差太远!")
        #     return True
        # if self.is_on_ground and self.steps > 100:  # 地面超过 100 步未起飞
        #     logger.warning("起飞超时!")
        #     return True
        if not self.is_on_ground and self.current_waypoint_idx >= len(self.waypoints):
            logger.info("All waypoints reached!")
            self._plot_trajectory()
            return True

        return False

